%% Saturn
warning off
clc;clear;
format longg
options_negx = odeset('RelTol',1e-13,'AbsTol',1e-13,'Events',@NegXcrossing);
options=odeset('RelTol',1e-13,'AbsTol',1e-13);
options_fmincon = optimoptions('fmincon', 'MaxFunctionEvaluations', 5000, ...
    'MaxIterations', 5000,'ConstraintTolerance',1e-6,'Algorithm','Interior-point','stepTolerance',1e-21);
%% Inputs.
G           = 6.674e-11 * ((1/1000)^3); % Gravitational parameters

mass_central = 1.989e30;
%[] Mass of central planet

mass_moon = 5.972e24;         %Callisto
%[] mass of moons

DU = 151.73e6;           %Callisto
%[km] Semi-major axis of Moons, used as distance units for each CRTBP
%environment.

moonName = {'Luna'};
%[] Name of the Moons

thetao = 0;
%[] Initial position of the moon relative to the first point of aries

GM_central = mass_central*G;
%[] Gravitational parameters

GM_moon = mass_moon*G;
%[] Gravitational parameters

N = length(mass_moon);
%[] Number of Moons

u = zeros(N,1);
for ii = 1:N
    u(ii) = GM_moon(ii)/(GM_moon(ii)+GM_central);
end
%[] Gravataional ratio

TU = zeros(N,1);
VU = zeros(N,1);
for ii = 1:N
    TU(ii) = sqrt(DU(ii)^3/GM_central);
    VU(ii) = DU(ii)/TU(ii);
end
%[] Time constant for each crtbp system

theta_dot = zeros(1,N);
for ii = 1:length(theta_dot)
    theta_dot(ii) = sqrt(GM_central*DU(ii))/DU(ii)^2;
end
%[] Theta dot for each planet.


planetNumb = 1;
[L1,L2,L3,L4,L5] = librationPoints(u(planetNumb));
L_ = [L1,L2,L3,L4,L5];
for ii = 1:length(L_)
    L_pos = L_(:,ii);
    J_L(ii) = jacobiConst(L_pos,zeros(3,1),u(planetNumb));
end

%% environment settings 
dt = 0.01;
startphase=0; % changing the starting position of the periodic
% orbit to show changes in the pole visibility based on the Satellite's position wrt the Sun's pole
rotationAxis = 7.25;
T1 = Rotation(cross([0;0;1],L_(:,4)),rotationAxis,'Degrees');
% OpenCRTBP_u(u)
%% Define inertial frame FNs
[xs,ys,zs] = ellipsoid(-0,0,0,696340,696340,696340,15);
s = surface(xs,ys,zs,'EdgeColor','flat','FaceAlpha',0.3,'FaceColor','k'); hold on;
rotate(s, [1 0 0], 7.25);
rotate(s, [0 0 1], 73.67+0.014*(2023-1850));
axis equal
FN_temp = s.VertexNormals;
[row,col,depth] = size(FN_temp);
FN_i = zeros(size(FN_temp));
for ii = 1:row
    for jj = 1:col
        FN_i(ii,jj,:)=T1*[FN_temp(ii,jj,1);...
            FN_temp(ii,jj,2);...
            FN_temp(ii,jj,3)];
    end
end
% p1 = [FN_i(10,10,1) FN_i(10,10,2) FN_i(10,10,3)]*696340;                         % First Point
% p2 = [FN_i(10,10,1) FN_i(10,10,2) FN_i(10,10,3)]*696340+100000;                         % Second Point
% dp = p2-p1;                         % Difference
% quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'k','LineWidth',2)
%%
% p1 = [0 0 0];                         % First Point
% p2 = [0 0 1e6];                         % Second Point
% dp = p2-p1;                         % Difference
% quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'k','LineWidth',2)
% text(p2(1),p2(2),p2(3), 'Z','FontSize',15,'BackgroundColor','none')

axis equal
p1 = [0 0 0];                         % First Point
p2 = [1.3e6 0 0];                         % Second Point
dp = p2-p1;                         % Difference
quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'k','LineWidth',2)
text(p2(1),p2(2),p2(3), 'X (♈)','FontSize',15,'BackgroundColor','none')

% p1 = [0 0 0];                         % First Point
% p2 = [0 1e6 0];                         % Second Point
% dp = p2-p1;                         % Difference
% quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'k','LineWidth',2)
% text(p2(1),p2(2),p2(3), 'Y','FontSize',15,'BackgroundColor','none')


%%
rotationAxis = 7.25;
T1 = Rotation([1;0;0],rotationAxis,'Degrees');
rotationAxis = 73.67+0.014*(2023-1850);
T2 = Rotation([0;0;1],rotationAxis,'Degrees');
T = T2*T1;

p1 = [0 0 0];                         % First Point
p2 = T*[0 0 1e6]';                         % Second Point
dp = p2-p1;                         % Difference
quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'r','LineWidth',2)
text(p2(1),p2(2),p2(3), '+Z (North pole)','FontSize',15,'BackgroundColor','none','Color','r')

p1 = [0 0 0];                         % First Point
p2 = T*[0 0 1e6]';                         % Second Point
dp = 5*(p2'-p1);                         % Difference
% quiver3(p1(1),p1(2),0,dp(1),dp(2),0,'b','LineWidth',2)
% text(p2(1),p2(2),p2(3), '+Z (North pole)','FontSize',15,'BackgroundColor','w','Color','b')

p1 = [0 0 0];                         % First Point
p2 = T*[0 0 -1e6]';                         % Second Point
dp = p2-p1;                         % Difference
quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'r','LineWidth',2)
text(p2(1),p2(2),p2(3), '-Z (South pole)','FontSize',15,'BackgroundColor','none','Color','r')

axis equal
p1 = [0 0 0];                         % First Point
p2 = T*[1e6 0 0]';                         % Second Point
dp = p2-p1;                         % Difference
quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'r','LineWidth',2)
text(p2(1),p2(2),p2(3), 'X','FontSize',15,'BackgroundColor','none','Color','r')
 
p1 = [0 0 0];                         % First Point
p2 = T*[0 1e6 0]';                         % Second Point
dp = p2-p1;                         % Difference
quiver3(p1(1),p1(2),p1(3),dp(1),dp(2),dp(3),'r','LineWidth',2)
text(p2(1),p2(2),p2(3), 'Y','FontSize',15,'BackgroundColor','none','Color','r')

%%


% [X,Y] = meshgrid(-1e6:100000:1e6);
% mesh(X,Y,zeros(size(X)),'LineStyle','-','FaceAlpha',0.1,'EdgeColor','none','FaceColor','k')
% hh2 = mesh(X,Y,zeros(size(X)),'LineStyle','-','FaceAlpha',0.1,'EdgeColor','none','FaceColor','r');
% rotate(hh2, [1 0 0], 7.25);
% rotate(hh2, [0 0 1], 73.67+0.014*(2023-1850));


%%
